#include <cv.h>
#include <algorithm>
#include <vector>
#include <math.h>
#include <highgui.h>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string.h>
//#include <imgproc.h>
//#include <cvcam.h>
using namespace cv;
using namespace std;

//colour numiration
//1 - white
//2 - yellow
//3 - red
//4 - blue
//5 - green
//6 - orange


//filtering parametrs
int thresh = 20, N = 7;
//window name
const char* wndname = "Capture";
//cube massive
int faces[6][9];
const string colors[] = {"white","yellow","red","blue","green","orange"};
//angle structure
static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}
double dist(pair<int,int> a, pair<int,int> b){
	return double((a.first-b.first)*(a.first-b.first) + (a.second-b.second)*(a.second-b.second));
}
//this void detects all rectangles on image
static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    squares.clear();

    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));
    pyrUp(pyr, timg, image.size());
    vector<vector<Point> > contours;

    for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        for( int l = 0; l < N; l++ )
        {
            if( l == 0 )
            {
                Canny(gray0, gray, 0, thresh, 5);
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                gray = gray0 >= (l+1)*255/N;
            }

            findContours(gray, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            for( size_t i = 0; i < contours.size(); i++ )
            {
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.02, true);

                if( approx.size() == 4 &&
                		fabs(contourArea(Mat(approx))) > 700 &&
                		fabs(contourArea(Mat(approx))) < 5000 &&
                    isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.15 )
                        squares.push_back(approx);
                }
            }
        }
    }
}


// the function draws all the squares in the image
static void drawSquares( Mat& image, const vector<vector<Point> >& squares )
{
    for( size_t i = 0; i < squares.size(); i++ )
    {

    	const Point* p = &squares[i][0];
        int n = (int)squares[i].size();
        int CenterX=0,CenterY=0;//finding centers of counters
        for(size_t j = 0; j < squares[i].size(); j ++){
        	CenterX += squares[i][j].x;
        	CenterY += squares[i][j].y;
        }
        CenterX /= squares[i].size();
        CenterY /= squares[i].size();
        polylines(image, &p, &n, 1, true, Scalar(0,255,0), 3, CV_AA);
    }

    imshow(wndname, image);
}
void sortCenters(vector<pair<int,int> > &centers){
	vector<pair<int,int> > copyv;
	int centerNum;
	int mind=1000000000;
	for(int i = 0; i < 9; i ++){
		copyv.push_back(centers[i]);
	}
	for(int i = 0; i < 9; i ++){
		int curDist=0;
		for(int j = 0; j < 9; j ++){
			if(j != i){
				curDist += dist(copyv[i],copyv[j]);
			}
		}
		if(curDist < mind){
			mind = curDist;
			centerNum = i;
		}
	}
	int maxDist = -1;
	vector<pair<double,int> > tmp;
	vector<pair<int,int> >ans;
	vector<double> dists;
	ans.assign(9,make_pair(0,0));
	ans[4] = centers[centerNum];
	int counter = 0;
	for(int i = 0; i < 9;i ++){
		if(i != centerNum){
			maxDist =  max(maxDist,int(dist(copyv[i],copyv[centerNum])));
			dists.push_back(dist(copyv[i],copyv[centerNum]));
			tmp.push_back(make_pair(atan2(double(copyv[i].second-copyv[centerNum].second),double(copyv[i].first - copyv[centerNum].first)),i));
			counter ++;
		}
	}
	sort(tmp.begin(),tmp.end());
	counter = 0;
	int i = 0;
	for(int i = 0;i< 8; i ++){
		tmp.push_back(tmp[i]);
	}
	int rightCorner = 0;
	for(int i = 0; i < 8 ; i ++){
		if(tmp[i].first > maxDist*1.2/sqrt(2)){
			rightCorner = i;
		}
	}
	ans[2] = copyv[tmp[rightCorner].second];
	ans[1] = copyv[tmp[rightCorner+1].second];
	ans[0] = copyv[tmp[rightCorner+2].second];
	ans[3] = copyv[tmp[rightCorner+3].second];
	ans[6] = copyv[tmp[rightCorner+4].second];
	ans[7] = copyv[tmp[rightCorner+5].second];
	ans[8] = copyv[tmp[rightCorner+6].second];
	ans[5] = copyv[tmp[rightCorner+7].second];
	for(int i = 0; i < 9; i ++){
		centers[i] = ans[i];
	}
}
bool isFace(vector<vector<Point> >&squares,vector<pair<int,int> > &centers ){ // checking if the image id a face of a cube
																			//and detects the cube's pieces
	bool res = true;
	//cout << "ololo"<< squares.size()<<endl;

	if(squares.size() >= 9){
		double maxlen = 0,len =0;
		for(size_t i = 0; i < squares.size(); i ++){
			int CenterX=0,CenterY=0;
			for(size_t j = 0; j < squares[i].size(); j ++){
			   	int x,y;
				x = squares[i][j].x;
				CenterX += x;
				y= squares[i][j].y;
				CenterY += y;

				if(j){
					x -= squares[i][j-1].x;
					y -= squares[i][j-1].y;
				}else{
					x -= squares[i][squares[i].size()-1].x;
					y -= squares[i][squares[i].size()-1].y;

			    }
				len = sqrt(double(x*x+y*y));
				if(maxlen < len){
					maxlen = len;
				}
			}
			CenterX /= squares[i].size();
			CenterY /= squares[i].size();
			bool alreadyIn = false;
			for(size_t j = 0; j < centers.size(); j ++){
				int x = centers[j].first;
				int y = centers[j].second;
				if(abs(CenterX-x) < 22 &&
					abs(CenterY-y) < 22){
						alreadyIn = true;
						break;

					}
			}
			if(!alreadyIn){
			  centers.push_back(make_pair(CenterX,CenterY));
			}
		}
		if(centers.size() != 9){
			return false;
		}
		for(size_t i = 1; i < centers.size(); i ++){
			for(size_t j = 0;j < i; j ++){
				double dx = centers[i].first - centers[j].first;
				double dy = centers[i].second - centers[j].second;
				double dist = sqrt(dx*dx+dy*dy);
				if(dist> maxlen*4.5){
					res = false;
					break;
				}
			}
			if(!res){
				break;
			}
		}
	}else{
		res = false;
	}
	return res;
}

//Is important in detection of colours
static void RGBtoHSV( double r, double g, double b, double *h, double *s, double *v )
{
	double Min, Max, delta;
	Min = min(min( r, g), b );
	Max = max(max( r, g), b );
	*v = Max;				// v
	delta = Max - Min;
	if( Max != 0 )
		*s = delta / Max;		// s
	else {
		// r = g = b = 0		// s = 0, v is undefined
		*s = 0;
		*h = -1;
		return;
	}
	if( r == Max )
		*h = ( g - b ) / delta;		// between yellow & magenta
	else if( g == Max )
		*h = 2 + ( b - r ) / delta;	// between cyan & yellow
	else
		*h = 4 + ( r - g ) / delta;	// between magenta & cyan
	*h *= 60;				// degrees
	if( *h < 0 )
		*h += 360;
}

//get colour name of image
static int getColour(Vec3b colour){
	double h,s,v;
	RGBtoHSV(colour[2],colour[1],colour[0],&h,&s,&v);
	//cout << h << " "<<s <<" "<<v << endl;

	if(h != h){
	//	cout << "nan";
		return 1;

    }
	if(s < 0.02){
		return 1;
	}
	if((h >= 45)&&(h <= 105)){
			return 2;
	}
	if((h >= 290)&&(h <= 350)){
			return 3;
	}
	if((h >= 175)&&(h <= 290)){
			return 4;
	}
	if((h >= 105)&&(h <= 195)){
			return 5;
	}
	if((h >= 350)||(h <= 45)){
			return 6;
	}
	return 0;



}
static int getFace(Mat &image){//returns a face of a cube
	vector<vector<Point> > squares;
	vector<pair<int,int> > centers;

	findSquares(image, squares);
    drawSquares(image, squares);
    bool isf = isFace(squares,centers);
    if(isf){
    	sort(centers.begin(),centers.end());
    	sortCenters(centers);
    	for(size_t i = 0; i < centers.size(); i ++){
    		int b=0,g=0,r=0;
    		int CenterX = centers[i].first;
    		int CenterY = centers[i].second;

    		for(int dy = -10; dy <= 10; dy ++){
    			for(int dx = -10; dx <= 10; dx ++){
    				Vec3b curColor = image.at<Vec3b>(abs(CenterY+dy), abs(CenterX + dx));
    				b +=curColor[0];
    				g +=curColor[1];
    				r += curColor[2];
    			}
    		}
    		b /= 21*21;
    		g /= 21*21;
    		r /= 21*21;
    		//cout << b << " "<< g << " "<<r << endl;

        	Vec3b curColour;
        	curColour[0] = b;
        	curColour[1] = g;
        	curColour[2] = r;
        	int color = getColour(curColour);
        	cout << colors[color-1]<<endl;
        }
    	cout << endl;
    	return 1;
    }else{
    	return 0;
    }
}

int main(int argc, char* argv[])
{
        freopen("input.txt","w",stdout);
	    CvCapture* capture = cvCreateCameraCapture(0);
	    assert( capture );
        double width = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);
        double height = cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);
        printf("[i] %.0f x %.0f\n", width, height );
        IplImage* frame=0;
        cvNamedWindow(wndname, CV_WINDOW_AUTOSIZE);
        //cvNamedWindow("debug", CV_WINDOW_AUTOSIZE);
        printf("[i] press Enter for capture image and Esc for quit!\n\n");
        int counter=0;
        char filename[512];
        int capturedFaces = 0;
        while(capturedFaces < 6){
                frame = cvQueryFrame( capture );
                //cvShowImage(wndname, frame);
                Mat pic(frame,1);
                for( int y = 0; y < pic.rows; y++ )
                    { for( int x = 0; x < pic.cols; x++ )
                         { for( int c = 0; c < 3; c++ )
                              {
                         pic.at<Vec3b>(y,x)[c] =
                         saturate_cast<uchar>( 1.2*( pic.at<Vec3b>(y,x)[c] )+20);
                             }
                    }
                    }
                int capturedFace = getFace(pic);
                //cout << capturedFace;
                if(capturedFace < 0){
                	cout << "Face is already captured! Turn to another face."<<endl;
                }else{
                    if(capturedFace > 0){
    //                	cout << "face captured"<<endl;
                    }
                }
                char c = cvWaitKey(33);
                if (c == 27) {
                        break;
                }
                else if(c == int('\n')) {
                        sprintf(filename, "Image%d.jpg", counter);
                        printf("[i] capture... %s\n", filename);
                        cvSaveImage(filename, frame);
                        counter++;
                }
        }
        cvReleaseCapture( &capture );
        cvDestroyWindow("capture");
        //cvDestroyWindow("debug");
        return 0;
}

